package novemberBot.sensor;

import lejos.nxt.SensorPort;
import lejos.nxt.TachoMotorPort;
import lejos.nxt.UltrasonicSensor;
import novemberBot.EnhancedMotor;
import novemberBot.FactoryFloorRobot;
import novemberBot.navigation.Odometer;
import novemberBot.utils.Trig;

/**Represents a motorised ultrasonic Sensor, extends thread so that it can sweep
 * constantly and still return the controller's Thread immediately 
 * */
public class MotorisedUsS extends Thread{
	/**Range to sweep in*/
	private static int[] range;
	/**speed at which the sensor sweeps in deg/s*/
	private static double sweepSpeed;
	/** true if MotorisedUsS is sweeping false if not*/
	private static boolean cSweeping=false;
	/**Motor controlling the sensor*/
	private static EnhancedMotor sensorMotor;
	/**Ultrasonic sensor*/
	private UltrasonicSensor us;
	/**ref to odometer to sync odo pos and sensor Orientation*/
	private static Odometer odo;
	
	
	/**Standard constructor
	 * @param motorPort - port for sensor motor
	 * @param gearRatio - ratio for gears from sensor to motor
	 * @param sensorPort - port for usSensor
	 * @param start - starts the thread or not*/
	public MotorisedUsS(TachoMotorPort motorPort, SensorPort sensorPort, double gearRatio, boolean start){
		sensorMotor=new EnhancedMotor(motorPort, gearRatio);
		sensorMotor.setAutoStop(true);
		us=new UltrasonicSensor(sensorPort);
		odo= FactoryFloorRobot.getInstance().getOdometer();
		if(start) this.start();
	}
	
	/**Sleeps the thread, waits for interrupt then performs last command and goes
	 * back to sleep*/
	@Override
	public void run(){
		while(true){
			try {
				Thread.sleep(60000);
			} catch (InterruptedException e) {
				if(cSweeping)runSweep();
			}
		}
	}
	/**pings the sensor, sleeps 50ms then returns distance and sensor position at ping time
	 * @return double[]{sensor measurement, x pos, y pos, sensor orientation}*/
	public double[] getDistnPos(){
		us.ping();
		double[] cPos = new double[3];
		odo.getPosition(cPos);
		cPos[2]+=Trig.fixAngle2(sensorMotor.getGearTachoCount());	
		try {
			Thread.sleep(30);
		} catch (InterruptedException e) {}
		return new double[]{us.getDistance(), cPos[0], cPos[1], Trig.fixAngle(cPos[2])};
	}
	
	
	/**sweeps back and forth for milis miliseconds between range[0] and range[1]
	 * @param range - range in which it sweeps, should be an array of length 2. 
	 * range[0] represents left boundary and range[1] the right
	 * @param sweepSpeed - speed in deg/s for sensor*/
	public void constantSweep(int[] range, double sweepSpeed){
		MotorisedUsS.range=range;
		MotorisedUsS.sweepSpeed=sweepSpeed;
		cSweeping=true;
		this.interrupt();		
	}
	/**pauses a constant sweep*/
	public void pauseSweep(){
		cSweeping=false;
		//sensorMotor.stop();

		//sensorMotor.stop();
		long wait=System.currentTimeMillis();
		while(sensorMotor.isMoving() &&(System.currentTimeMillis()-wait)<3000) Thread.yield();
		sensorMotor.stop();
	}
	/**resume a paused sweep pattern*/
	public void resumeSweep(){
		cSweeping=true;
		this.interrupt();
		long wait=System.currentTimeMillis();
		while(!sensorMotor.isMoving() && System.currentTimeMillis()-wait<1000) Thread.yield();
	}
	/**called by run() performs the constant sweep*/
	private void runSweep(){
		sensorMotor.setGearSpeed(sweepSpeed);
		try {
			Thread.sleep(300);
		} catch (InterruptedException e) {}
		
		while(cSweeping){
			sensorMotor.rotateGearTo(range[0], false, true);
			if(!cSweeping)break;
			sensorMotor.rotateGearTo(range[1], false, true);
			
		}
		
	}
	/**moves the sensor to the desired angle at the desired speed
	 * @param angle - angle to sweep to
	 * @param speed - speed to sweep in deg/sec for angle param
	 * @param immediateReturn - returns before or after motion*/
	public void sweepTo(double angle, double speed, boolean immediateReturn){
		long wait=System.currentTimeMillis();
		
		
		while(sensorMotor.isMoving() && (System.currentTimeMillis()-wait)<1000) Thread.yield();
		sensorMotor.stop();
		sensorMotor.setGearSpeed(speed);
		sensorMotor.rotateGearTo(angle, immediateReturn, true);
	}
	
	/**not really in use yet not tested should tell in real time weather a motor is stuck or not
	 * by comparing the motor mode to the tacho variation rate*/
	public boolean isStuck(){
		return sensorMotor.isMoving() && !sensorMotor.isReallyMoving();
	}
	
	
	//accessors
	/**accessor for sweeping boolean
	 * @return true if sensor is currently busy sweeping, false if it is not and ready for a new command*/
	public boolean isSweeping(){
		return sensorMotor.isMoving() || cSweeping ;
	}
	
	/**accessor for sensorMotor*/
	public EnhancedMotor getSensorMotor(){
		return sensorMotor;
	}
	


}
